/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <math.h>
#include <SDL/SDL.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <string>
#include <vector>
#include <assert.h>
#include "CSDLOpenGLCamera.h"  // Camera handling (move, resize etc)
#include "GLUtils.h"           // Plenty of helpful GL draw functions

///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/// GLOBALS
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
int Window_ID1;
COpenGLCamera cam;                   // All camera handling

struct trajectoryPoint{
		float x,y,z;
		int color;
		
		trajectoryPoint(){
				set(0,0,0,0);
		}
		trajectoryPoint(float _x,float _y, float _z, int c){x=_x;y=_y;z=_z;color=c;}
		void set(float _x,float _y, float _z, int c){x=_x;y=_y;z=_z;color=c;}	
};

struct RGBPoint{
	float x,y,z;
	int r,g,b;
	
	RGBPoint(){
		set(0,0,0,0,0,0);
	}
	RGBPoint(float _x,float _y, float _z, unsigned char r_,unsigned char g_, unsigned char b_){
		x=_x;y=_y;z=_z;
		r=r_;g=g_;b=b_;
	}
	void set(float _x,float _y, float _z, unsigned char r_,unsigned char g_, unsigned char b_){
		x=_x;y=_y;z=_z;
		r=r_;g=g_;b=b_;
	}	
};


struct line3d{
		float x1,y1,x2,y2,h;
		glutgui::Color col;
		line3d(){
				set(0,0,0,0,0);
		}
		line3d(float _x1,float _y1,float _x2,float _y2,float height){
				x1=_x1;x2=_x2;y1=_y1;y2=_y2;h=height;
		}
		void set(float _x1,float _y1,float _x2,float _y2,float height){
				x1=_x1;x2=_x2;y1=_y1;y2=_y2;h=height;
		}
				
};

static std::vector< RGBPoint > points;
static std::vector< line3d > lines;
static int displayListId=-1;
static std::vector<glutgui::Color> colors;


void generateColors(){
	colors.push_back(glutgui::Color(0,1,0));		
	colors.push_back(glutgui::Color(0,0,1));
	colors.push_back(glutgui::Color(0,1,1));
	colors.push_back(glutgui::Color(1,0,0));
	colors.push_back(glutgui::Color(1,1,0));
	colors.push_back(glutgui::Color(1,0,1));
}


GLvoid DrawBox(float x, float y, float z)
{
	// dPrint(0, "%lf %lf %lf %lf %lf %lf", x,y,z,r,g,b);
	
	//float mcolor[3] = {r, g, b };
	//glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);
	float cubesize = 0.025f;
	
	
	glBegin(GL_QUADS);
	glNormal3f( 0.0f, 0.0f, 1.0f);
	glVertex3f( cubesize+x, cubesize+y, cubesize+z); glVertex3f(-cubesize+x, cubesize+y, cubesize+z);
	glVertex3f(-cubesize+x,-cubesize+y, cubesize+z); glVertex3f( cubesize+x,-cubesize+y, cubesize+z);
	
	
	glNormal3f( 0.0f, 0.0f,-1.0f);
	glVertex3f(-cubesize+x,-cubesize+y,-cubesize+z); glVertex3f(-cubesize+x, cubesize+y,-cubesize+z);
	glVertex3f( cubesize+x, cubesize+y,-cubesize+z); glVertex3f( cubesize+x,-cubesize+y,-cubesize+z);
	
	glNormal3f( 0.0f, 1.0f, 0.0f);
	glVertex3f( cubesize+x, cubesize+y, cubesize+z); glVertex3f( cubesize+x, cubesize+y,-cubesize+z);
	glVertex3f(-cubesize+x, cubesize+y,-cubesize+z); glVertex3f(-cubesize+x, cubesize+y, cubesize+z);
	
	glNormal3f( 0.0f,-1.0f, 0.0f);
	glVertex3f(-cubesize+x,-cubesize+y,-cubesize+z); glVertex3f( cubesize+x,-cubesize+y,-cubesize+z);
	glVertex3f( cubesize+x,-cubesize+y, cubesize+z); glVertex3f(-cubesize+x,-cubesize+y, cubesize+z);
	
	glNormal3f( 1.0f, 0.0f, 0.0f);
	glVertex3f( cubesize+x, cubesize+y, cubesize+z); glVertex3f( cubesize+x,-cubesize+y, cubesize+z);
	glVertex3f( cubesize+x,-cubesize+y,-cubesize+z); glVertex3f( cubesize+x, cubesize+y,-cubesize+z);
	
	glNormal3f(-1.0f, 0.0f, 0.0f);
	glVertex3f(-cubesize+x,-cubesize+y,-cubesize+z); glVertex3f(-cubesize+x,-cubesize+y, cubesize+z);
	glVertex3f(-cubesize+x, cubesize+y, cubesize+z); glVertex3f(-cubesize+x, cubesize+y,-cubesize+z);
	
	
	glEnd();
	
	
	
	
}



void generateDisplayList(){
		displayListId = glGenLists(1); 
		
		glNewList(displayListId, GL_COMPILE);
		
		
		glPointSize(5.0);
		//glPointParameterf(	GL_POINT_SIZE_MIN, 1.0);
		glBegin(GL_POINTS);
		for(int i=0;i<points.size();i++){
			glColor3f((float)points[i].r/255.0, (float)points[i].g/255.0, (float)points[i].b/255.0 );	
			//glColor3f(1.0, 1.0, 1.0 );	
			//glutils::drawBox(0.2, 0.2, trajectory[i].z+0.2, trajectory[i].x, trajectory[i].y, trajectory[i].z-trajectory[i].z/2.0,colors[trajectory[i].color]);
			
			//DrawBox(points[i].x, -points[i].z, -points[i].y);
			
			glVertex3f(points[i].x, -points[i].z, -points[i].y);
			
			
			//glVertex3f(points[i].x, points[i].y, points[i].z);
				//glVertex3f(trajectory[i].x, 0,-trajectory[i].y);
//glVertex3f(trajectory[i].x, trajectory[i].z,-trajectory[i].y);
				glutgui::Color c(points[i].r, points[i].g, points[i].b );
				//glutils::drawBox(0.1, 0.1, 0.1, points[i].x, points[i].y, points[i].z, c);
		}
		glEnd();	
		glutils::drawGrid(10.0, 100, 0,  colors[0]);
		//glutils::drawGrid(1.0, 100, 4.2,  colors[1]);
		glEndList();
}

/**
* An example of how to use the 3D visualization stuff. 
* GLUtils.h
*/
void do3Doperations(){
	
		if(displayListId > 0){
				glCallList(displayListId);
		}else{
			fprintf(stderr,"generating displaylist....");
				generateDisplayList();
				fprintf(stderr,"done!\n");
		}
		
	
	
	///
	/*
	glutils::line3D(-10, -10,1, -10, 10, 1 , glutgui::Color(0,1,1));
	glutils::drawWall(-11, -11, 0, 0, 0.1, 2);
	*/
}


/**
* Called from the render func to draw all the objects
* 
*/
void drawObjects(){
	static int counter=0;
	counter++;
	
	///Enable if you want to see amazing 3D :)
	do3Doperations();
	
	assert(glGetError() == GL_NO_ERROR);
}
/**
* REnder function. Draws everything that is visible.
*/
void renderScene(void) {
	cam.updateCamera();

	float Light_Ambient[]=  { 0.1f, 0.1f, 0.1f, 1.0f };
	float Light_Diffuse[]=  { .6f, .6f, .6f, 1.0f }; 
	float Light_Position[]= { 0.0f, 50.0f, 0.0f, 1.0f };
	float Light_Specular[]= { 5.0f, 5.0f, 5.0f, 10.0f };
	
	float c[4];
	glGetFloatv(GL_CURRENT_COLOR,c);

	cam.setCamera(); ///< sets up the view
	
	glPushMatrix();
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_ACCUM_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
	//glEnable(GL_POINT_SMOOTH);
	
	/*glEnable(GL_LIGHTING);
	glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
	glEnable(GL_COLOR_MATERIAL);
		
	glLightfv(GL_LIGHT1, GL_POSITION, Light_Position);
	glLightfv(GL_LIGHT1, GL_AMBIENT,  Light_Ambient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE,  Light_Diffuse); 
	glLightfv(GL_LIGHT1, GL_SPECULAR,  Light_Specular); 
	glEnable (GL_LIGHT1); 

	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LEQUAL);							// The Type Of Depth Testing To Do
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);			// Really Nice Perspective Calculations		
	
	glPushMatrix();
	glPushAttrib(GL_ALL_ATTRIB_BITS);
	glEnable (GL_BLEND); 
	glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	*///glColor4f(0,1,1,0.1);
	
	glColor4f(0.8,0.8,1.0,0.9);
	glDisable (GL_BLEND); 
	glDisable(GL_TEXTURE_2D);
	
	/*
	glBegin(GL_QUADS);
	glVertex3f(-100,-.25,100);
	glVertex3f(100,-.25,100);
	glVertex3f(100,-.25,-100);
	glVertex3f(-100,-.25,-100);
	glEnd();
	*/
	glDisable (GL_BLEND); 
	//glPopAttrib();
	//glPopMatrix();
	

	glPushAttrib(GL_ALL_ATTRIB_BITS);
	glPushMatrix();
	glDisable(GL_TEXTURE_2D);
	drawObjects();
	
	glPopMatrix();
	glPopAttrib();
	
	glPopMatrix();
	glFlush();
	SDL_GL_SwapBuffers();	
}


//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////

/**
* Initializes the example panel
* Headers:
* GUIPanel.h
* Button.h
* RGBScreen.h
* RadioButtonGroup.h
* TextureLabel.h
**/
void init(){
  glEnable(GL_DEPTH_TEST);
  glEnable( GL_TEXTURE_2D ); 
	//cam.setCameraPose(0, -8,-20, 0, 25);
	cam.setCameraPose(0, -15,-10, 0, 35);
	cam.setResolution(0.2, 1.5);

}

/**
* Main
**/
int main(int argc, char* argv[])
{
	SDL_Surface *SDLscreen;
	int done;
	Uint8 *keys;	
	int videoFlags; ///<Flags to pass to SDL_SetVideoMode
	const SDL_VideoInfo *videoInfo;
	/* whether or not the window is active */
	int isActive = true;
	
	if(argc < 2){
		fprintf(stderr,"----------------------------------------------------------------------------\n");
		fprintf(stderr,"\tPlots linemaps in 3D with 3D trajectory");
		fprintf(stderr,"\tLine maps are given in 2D format with height (file input)\n");
		fprintf(stderr,"\tThe trajectory is given in format [x y z int] where int is an integer [0,1,2] (file input)\n");
		fprintf(stderr,"\tThe integer defines the plot color \n");
		fprintf(stderr,"\n\t\tUsage: ./plot3DmapAndPath [point_file]\n");
		fprintf(stderr,"\t\t \n");
		fprintf(stderr,"----------------------------------------------------------------------------\n");
		exit(1);
	}
	
	std::string pathfilename=argv[1];
	
	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////
	/// READ TRAJECTORY FILE
	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////
	FILE *fpath=fopen(pathfilename.c_str(),"rt");
	if(fpath==NULL){
		fprintf(stderr,"Unable to open %s\n",pathfilename.c_str());
		exit(1);
	}
	RGBPoint tp;
	
	fprintf(stderr,"Loading.... ");
	while(!feof(fpath)){
		
			fscanf(fpath,"%f %f %f %d %d %d\n",&tp.x,&tp.y,&tp.z,&tp.r,&tp.g,&tp.b);
			
			//fprintf(stderr,"%f %f %f %d %d %d\n",tp.x,tp.y,tp.z,tp.r,tp.g,tp.b);
			points.push_back(tp);
	}
	fprintf(stderr,"Done - Got %d points!!\n",points.size());
	
	fclose(fpath);
	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////
	generateColors();
	
	SDL_Init(SDL_INIT_VIDEO);
	if ( SDL_Init( SDL_INIT_VIDEO ) < 0 ){
		fprintf( stderr, "Video initialization failed: %s\n",SDL_GetError( ) );
		exit(1);
	}

	/* Fetch the video info */
	videoInfo = SDL_GetVideoInfo( );

	if ( !videoInfo ){
		fprintf( stderr, "Video query failed: %s\n", SDL_GetError( ) );
		exit( 1 );
	}

	/* the flags to pass to SDL_SetVideoMode */
	videoFlags  = SDL_OPENGL;          /* Enable OpenGL in SDL */
	videoFlags |= SDL_GL_DOUBLEBUFFER; /* Enable double buffering */
	videoFlags |= SDL_HWPALETTE;       /* Store the palette in hardware */
	videoFlags |= SDL_RESIZABLE;       /* Enable window resizing */

	/* This checks to see if surfaces can be stored in memory */
	if ( videoInfo->hw_available ) videoFlags |= SDL_HWSURFACE;
	else videoFlags |= SDL_SWSURFACE;

	/* This checks if hardware blits can be done */
	if ( videoInfo->blit_hw ) videoFlags |= SDL_HWACCEL;
	SDL_GL_SetAttribute( SDL_GL_DOUBLEBUFFER, 1 );

	/* get a SDL surface */
	SDLscreen = SDL_SetVideoMode( 800,600, 24, videoFlags);

	/* Verify there is a surface */
	if ( !SDLscreen ){
		fprintf( stderr,  "Video mode set failed: %s\n", SDL_GetError( ) );
		exit( 1 );
	}
	
	SDL_WM_SetCaption("SDL/OPEN GL GUI - JS/2008", "SDL/OPEN GL GUI - JS/2008");

	init();
	cam.resizeFunc(SDLscreen->w, SDLscreen->h);
	done = 0;
	while ( ! done ) {
		SDL_Event event;
		while ( SDL_PollEvent(&event) ) {
			switch(event.type) {
				case SDL_ACTIVEEVENT:
			  	//if ( event.active.gain == 0 ) isActive = false;
					//else isActive = true;
					break;
				case SDL_VIDEORESIZE:
					SDLscreen = SDL_SetVideoMode(event.resize.w, event.resize.h, 32,
																		SDL_OPENGL|SDL_RESIZABLE);
					if ( SDLscreen ) {
						cam.resizeFunc(SDLscreen->w, SDLscreen->h);
					} else {
            /* Uh oh, we couldn't set the new video mode?? */;
					}
					break;
				case SDL_QUIT: done = 1;break;
				case SDL_KEYDOWN:
				case SDL_KEYUP:
					cam.cameraKeyFunc(event.key); ///<Camera handling
					break;
        case SDL_MOUSEBUTTONDOWN:
					if(event.button.button == SDL_BUTTON_LEFT)
          break;     
        case SDL_MOUSEBUTTONUP: 
					break;
				case SDL_MOUSEMOTION:
					break;
				case SDL_JOYAXISMOTION:	break;
				case SDL_JOYBALLMOTION: break;
				case SDL_JOYHATMOTION: break;
				case SDL_JOYBUTTONDOWN: break;
				case SDL_JOYBUTTONUP: break;
					
			}
		}
		keys = SDL_GetKeyState(NULL);

		if ( keys[SDLK_ESCAPE] ) {
			done = 1;
		}
		if(keys[SDLK_F1]){ ///Bring panel to camera
		}
		if(isActive)renderScene();
		else usleep(100);
		usleep(5000);
	}	SDL_Quit();
	return 0;
}


